// Warp.cpp: implementation of the CWarp class.
//
//////////////////////////////////////////////////////////////////////

#include "stdAfx.h"
//#include "DIBTest.h"
#include "Warp.h"
#include "Math/matrix.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CWarp::CWarp()
{
}

CWarp::~CWarp()
{	
}

CMLS::CMLS(vector<CPoint> &CtrPs,vector<CPoint> &CtrQs)
{
	if(CtrPs.size()!=CtrQs.size()){
		MSG_BOX_INFO("CMLS, the # source point is not equal to the # destination point");
		return;
	}
	m_Src.clear();	m_Dst.clear();
	m_Src.resize(CtrPs.size());
	m_Dst.resize(CtrPs.size());
	for (unsigned i=0; i<CtrPs.size(); ++i){
		m_Src[i] = Vector2(CtrPs[i].x, CtrPs[i].y);
		m_Dst[i] = Vector2(CtrQs[i].x, CtrQs[i].y);
	}	
}

CMLS::CMLS(vector<Vector2> &CtrPs,vector<Vector2> &CtrQs)
{
	if(CtrPs.size()!=CtrQs.size()){
		MSG_BOX_INFO("CMLS, the # source point is not equal to the # destination point");
		return;
	}
	m_Src.clear();	m_Dst.clear();
	m_Src = CtrPs;
	m_Dst = CtrQs;	
}

Vector2 CMLS::Warping(Vector2 point)
{
		int nSize=m_Src.size();
		//compute weight
		vector<double> Weight;
		for (int i1=0;i1<nSize;i1++)
		{
			double dtemp=( pow(	static_cast<double>(point.x-m_Src[i1].x), 2)+
										   pow(	static_cast<double>(point.y-m_Src[i1].y), 2) );

			if(dtemp==0.0)
			{
				dtemp=dtemp+0.0000001;
			}
			
			double temp=1/dtemp;

			Weight.push_back(temp);
		}
		//compute pStar,QStar
		double dSumW=0;
		double dSumPx=0,dSumPy=0;
		double dSumQx=0,dSumQy=0;
		for (int i2=0;i2<nSize;i2++)
		{
			dSumW=dSumW+Weight[i2];
			dSumPx=dSumPx+Weight[i2]*(double)m_Src[i2].x;
			dSumPy=dSumPy+Weight[i2]*(double)m_Src[i2].y;
			dSumQx=dSumQx+Weight[i2]*(double)m_Dst[i2].x;
			dSumQy=dSumQy+Weight[i2]*(double)m_Dst[i2].y;
		}
		Vector2 PStar,QStar;
		PStar.x=(dSumPx/dSumW);
		PStar.y=(dSumPy/dSumW);
		QStar.x=(dSumQx/dSumW);
		QStar.y=(dSumQy/dSumW);

		vector<Vector2> PHat,QHat;
		for (int i3=0;i3<nSize;i3++)
		{
			Vector2 tempP;
			tempP.x=m_Src[i3].x-PStar.x;
			tempP.y=m_Src[i3].y-PStar.y;
			PHat.push_back(tempP);
			Vector2 tempQ;
			tempQ.x=m_Dst[i3].x-QStar.x;
			tempQ.y=m_Dst[i3].y-QStar.y;
			QHat.push_back(tempQ);
		}
		//compute the inverse Matrix
		matrix2 Mat1={0,0,0,0};
		for (int i4=0;i4<nSize;i4++)
		{
			Mat1.c11=Mat1.c11+Weight[i4]* pow(	static_cast<double>(PHat[i4].x), 2 );
			Mat1.c12=Mat1.c12+Weight[i4]*(double)(PHat[i4].x*PHat[i4].y);
			Mat1.c21=Mat1.c12;
			Mat1.c22=Mat1.c22+Weight[i4]* pow( static_cast<double>(PHat[i4].y), 2 );
		}

		double tempM=Mat1.c11*Mat1.c22-Mat1.c12*Mat1.c21;
		matrix2 MatInver;
		MatInver.c11=Mat1.c22/tempM;
		MatInver.c12=-Mat1.c12/tempM;
		MatInver.c21=-Mat1.c21/tempM;
		MatInver.c22=Mat1.c11/tempM;

		//compute the scale Aj
		vector<double> dScale;
		for (int i5=0;i5<nSize;i5++)
		{
			double tempS;
			tempS=((double)(point.x-PStar.x)*MatInver.c11+(double)(point.y-PStar.y)*MatInver.c21)*Weight[i5]*(double)PHat[i5].x
				+((double)(point.x-PStar.x)*MatInver.c12+(double)(point.y-PStar.y)*MatInver.c22)*Weight[i5]*(double)PHat[i5].y;
			dScale.push_back(tempS);
		}
		//compute the New Point
		Vector2 NewPoint;
		for (int i6=0; i6<nSize; i6++)
		{
			NewPoint.x=NewPoint.x+(int)(dScale[i6]*QHat[i6].x);
			NewPoint.y=NewPoint.y+(int)(dScale[i6]*QHat[i6].y);
		}
		NewPoint.x=NewPoint.x+QStar.x;
		NewPoint.y=NewPoint.y+QStar.y;
		return NewPoint;	
}



CIDW::CIDW(vector<CPoint> &CtrPs,vector<CPoint> &CtrQs)
{
	if(CtrPs.size()!=CtrQs.size())
		return;
	m_Src.resize(CtrPs.size());
	m_Dst.resize(CtrQs.size());
	for (unsigned i=0; i<CtrPs.size(); ++i){
		m_Src[i].x = CtrPs[i].x;	m_Src[i].y = CtrPs[i].y;
		m_Dst[i].x = CtrQs[i].x;	m_Dst[i].y = CtrQs[i].y;
	}
	ComputeMatrix();
}

CIDW::CIDW(vector<Vector2> &CtrPs,vector<Vector2> &CtrQs)
{
	m_Src=CtrPs;
	m_Dst=CtrQs;
	ComputeMatrix();
}

Vector2 CIDW::Warping(Vector2 point)
{
	int nSize=m_Src.size();
	vector<double> weight;	
	vector<double> dIndis;
	double dSumIndis=0;
	for (int i=0;i<nSize;i++)
	{
		double dTemp=InDistance(m_Src[i],point);
		dIndis.push_back(dTemp);
		dSumIndis+=dTemp;
	}
	for (int j=0;j<nSize;j++)
	{
		
		double  dWeight=dIndis[j]/dSumIndis;
		weight.push_back(dWeight);
	}

	Vector2 NewPoint(0,0);
	for (int k=0;k<nSize;k++)
	{
		
		NewPoint.x+=(int)(weight[k]*(m_Mat[k].c11*(point.x-m_Src[k].x)+m_Mat[k].c12*(point.y-m_Src[k].y)+m_Dst[k].x));
		NewPoint.y+=(int)(weight[k]*(m_Mat[k].c21*(point.x-m_Src[k].x)+m_Mat[k].c22*(point.y-m_Src[k].y)+m_Dst[k].y));
		
	}
	return NewPoint;
}

double CIDW::InDistance(Vector2 pointP, Vector2 point)
{
	double dTemp	=	pow( static_cast<double>(pointP.x-point.x), 2)+
									pow( static_cast<double>(pointP.y-point.y), 2) ;
	if (dTemp==0)
	{
		dTemp+=0.0000001;
	}
	double delta=1/dTemp;
	return delta;
}

matrix2 CIDW::ComputeMatrixD(Vector2 pointP, Vector2 pointQ)
{
	int nSize=m_Src.size();
	vector<double> weight;
	for (int i=0;i<nSize;i++)
	{
		double dInDis=InDistance(pointP,m_Src[i]);
		weight.push_back(dInDis); 
	}

	matrix2 ma1={0,0,0,0};
	matrix2 ma2={0,0,0,0};
	for (int j=0;j<nSize;j++)
	{
		ma1.c11+=weight[j]	*	pow( static_cast<double>(m_Src[j].x-pointP.x) , 2 );
		ma1.c12+=weight[j]	*	(double)((m_Src[j].x-pointP.x)*(m_Src[j].y-pointP.y));
		ma1.c22+=weight[j]	*	pow( static_cast<double>(m_Src[j].y-pointP.y) , 2 );
	
		ma2.c11+=weight[j]*(double)((m_Dst[j].x-pointQ.x)*(m_Src[j].x-pointP.x));
		ma2.c12+=weight[j]*(double)((m_Dst[j].x-pointQ.x)*(m_Dst[j].y-pointQ.y));
		ma2.c21+=weight[j]*(double)((m_Dst[j].y-pointQ.y)*(m_Src[j].x-pointP.x));
		ma2.c22+=weight[j]*(double)((m_Dst[j].y-pointQ.y)*(m_Src[j].y-pointP.y));		

	}
	double d11,d12,d21,d22;
	double dTemp;
	dTemp=ma1.c12*ma1.c12-ma1.c11*ma1.c22;
	d11=(ma1.c12*ma2.c12-ma1.c22*ma2.c11)/dTemp;
	d12=(ma1.c12*ma2.c11-ma1.c11*ma2.c12)/dTemp;
	d21=(ma1.c12*ma2.c22-ma1.c22*ma2.c21)/dTemp;
	d22=(ma1.c12*ma2.c21-ma1.c11*ma2.c22)/dTemp;

	matrix2 ma;
	ma.c11=d11;
	ma.c12=d12;
	ma.c21=d21;
	ma.c22=d22;
	return ma;
}

void CIDW::ComputeMatrix()
{
	int nSize=m_Src.size();
	matrix2 *ma=new matrix2[nSize];
	for (int i=0;i<nSize;i++)
	{
		ma[i]=ComputeMatrixD(m_Src[i],m_Dst[i]);
	}
	m_Mat=ma;
}
CIDW::~CIDW()
{
	SAFEDELETES(m_Mat);
}


CRBF::CRBF(vector<CPoint> &CtrPs,vector<CPoint> &CtrQs)
{

	if(CtrPs.size()!=CtrQs.size())
		return;
	m_Src.resize(CtrPs.size());
	m_Dst.resize(CtrQs.size());
	for (unsigned i=0; i<CtrPs.size(); ++i){
		m_Src[i].x = CtrPs[i].x;	m_Src[i].y = CtrPs[i].y;
		m_Dst[i].x = CtrQs[i].x;	m_Dst[i].y = CtrQs[i].y;
	}
	ComputeCoeffi();
}


CRBF::CRBF(vector<Vector2>& src, vector<Vector2>& dst, double _fR)
{
	m_fR = _fR;
	m_Src.clear();	m_Dst.clear();
	m_Src = src;
	m_Dst = dst;
	ComputeCoeffi();
}

Vector2 CRBF::Warping(Vector2 point)
{
	 Vector2 NewPoint(0,0);
	int nSize=m_Src.size();
	for (int i=0;i<nSize;i++)
	{
		double dRadial=ComputeRadial(m_Src[i],point);
		NewPoint.x+=m_solux[i]*dRadial;
		NewPoint.y+=m_soluy[i]*dRadial;
	}
	NewPoint.x+=m_solux[nSize]*point.x+m_solux[nSize+1]*point.y+m_solux[nSize+2];
	NewPoint.y+=m_soluy[nSize]*point.x+m_soluy[nSize+1]*point.y+m_soluy[nSize+2];
	return NewPoint;
}

double CRBF::ComputeRadial(Vector2 CtrP,Vector2 point)
{	
	double dTemp  =	pow(static_cast<double>(CtrP.x-point.x), 2 ) + pow( static_cast<double>(CtrP.y-point.y),2 )+pow(m_fR,2);
	double dRadial= sqrt(pow(dTemp,-1));
	return dRadial;
}

void CRBF::ComputeCoeffi()
{
	int nSize=m_Src.size();
	int N=nSize+3;	
	double **dCoeff=new double* [N];
	for (int i1=0;i1<N;i1++)
	{
		dCoeff[i1]=new double [N];
	}
	for (int i=0;i<N;i++)
		for (int j=0;j<N;j++)
		{
			if(i<=j)
			{
				if (j<N-3)
				{
					dCoeff[i][j]=ComputeRadial(m_Src[j],m_Src[i]);
				}
				else
				{
					if (i<N-3)
					{
						if (j==N-3)
						{
							dCoeff[i][j]=m_Src[i].x;
						}
						else if (j==N-2)
						{
							dCoeff[i][j]=m_Src[i].y;	
						}
						else
							dCoeff[i][j]=1;
					}
					else
						dCoeff[i][j]=0;
					
				}
			}
			else
				dCoeff[i][j]=dCoeff[j][i];			
		}
		RowMat<double>  rowc(N,N);
		CSRMatrix<double> MatC;
		for (int l=0;l<N;l++)
			for (int k=0;k<N;k++)
			{
				rowc[l][k]=dCoeff[l][k];
				//printf("%f     ",rowc[l][k]);
			}
		CreateCSRMatrixFromRowMap(MatC,rowc);
		double *bx=new double[N];
		double *by=new double[N];
		for (int i3=0;	i3 < static_cast<int>( m_Dst.size() );	i3++)
		{
			bx[i3]=m_Dst[i3].x;
			by[i3]=m_Dst[i3].y;
		}
		bx[N-1]=bx[N-2]=bx[N-3]=0;
		by[N-1]=by[N-2]=by[N-3]=0;
		
		double *x=new double[N];
		double *y=new double[N];
		MklSolveSparseSystem(MatC,bx,x);
		MklSolveSparseSystem(MatC,by,y);
		m_solux=x;
		m_soluy=y;	
}
CRBF::~CRBF()
{
	SAFEDELETES(m_solux);
	SAFEDELETES(m_soluy);
}